﻿#include "dmc_b140_m.h"
#include <QDebug>
#include <QString>
#include <QMessageBox>

const int BUF_SIZE = 40;

DMC_B140_M::DMC_B140_M(QObject *parent): MotionController(parent)
{
    m_socket = new QTcpSocket(this);
    m_ipAddress = "";
    m_portNum = 0;
    m_isInit = false;

    m_speed.resize(4);
    m_EncodePos.resize(4);
    m_TheoryPos.resize(4);
    m_IO.resize(8);
    for(int i = 0; i < 4; i++)
    {
        m_speed[i] = 0;
        m_EncodePos[i] = 0;
        m_TheoryPos[i] = 0;
    }

    connect(m_socket, SIGNAL(connected()), this, SLOT(initBasicPara()));
    connect(m_socket, SIGNAL(disconnected()), this, SLOT(disconnected()));
}

bool DMC_B140_M::cmdV(QString cmd)
{
//    qDebug() << "DMC_B140_M:" << cmd;
    if(m_socket->isWritable())
    {
        m_socket->write(cmd.toLocal8Bit());
        if(!m_socket->waitForReadyRead(100))
        {
            return false;
        }
        m_socket->readAll();
    }
    return true;
}

bool DMC_B140_M::cmdD(QString cmd, double *value)
{
    QString data;
    if(m_socket->isWritable())
    {
        m_socket->write(cmd.toLocal8Bit());
        if(!m_socket->waitForReadyRead(100))
        {
            return false;
        }
        data = m_socket->readAll();
    }
    bool ok;
    data.remove(QChar(':'));
    *value = data.toDouble(&ok);
    return ok;
}

bool DMC_B140_M::cmdI(QString cmd, int *value)
{
//    qDebug() << "DMC_B140_M:" << cmd;
    QString data;
    if(m_socket->isWritable())
    {
        m_socket->write(cmd.toLocal8Bit());
        if(!m_socket->waitForReadyRead(100))
        {
            return false;
        }
        data = m_socket->readAll();
        //qDebug() << data;
    }
    bool ok;
    data.remove(QChar(':'));
    *value = data.toInt(&ok);
    //qDebug() << data << ", " << *value;
    return ok;
}

bool DMC_B140_M::isMoving(unsigned short axis)
{
    double value;
    QString cmd = QString("MG _BG%1\r").arg(static_cast<char>(axis + 'A'));
    cmdD(cmd, &value);
    //qDebug() << "isMoving:" << value;
    return (value == 1.0);
}

bool DMC_B140_M::cmdI4(QString cmd, int *value)
{
//    qDebug() << "DMC_B140_M:" << cmd;
    m_socket->write(cmd.toLocal8Bit());
    if(!m_socket->waitForReadyRead(100))
    {
        return false;
    }
    else
    {
        QString data = m_socket->readAll();
        data.remove(QChar(':'));
        QStringList pos = data.split(',');
        if(pos.size() == 4)
        {
            for(int i = 0; i < 4; i++)
            {
                value[i] = pos[i].toInt();
            }
            return true;
        }
    }
    return false;
}

bool DMC_B140_M::cmdB8(QString cmd, bool *value)
{
    //qDebug() << "DMC_B140_M:" << cmd;
    m_socket->write(cmd.toLocal8Bit());
    if(!m_socket->waitForReadyRead(100))
    {
        return false;
    }
    else
    {
        QString data = m_socket->readAll();
        data.remove(QChar(':'));
        int ret = data.toInt();
        //qDebug() << "ret:" << ret;
        if(0 <= ret && ret <= 255)
        {
            for(int i = 0; i < 8; i++)
            {
                value[i] = (ret >> i) & 1;
                //qDebug() << "value " << i << ":" <<value[i];
            }
            return true;
        }
    }
    return false;
}

void DMC_B140_M::goHome(unsigned short axis, int speed)
{
    QString cmd = QString("SP%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(speed);
    cmdV(cmd);
    if(axis == AXIS_Z)
    {
        cmdV("CN,1\r");
    }
    else
    {
        cmdV("CN,-1\r");
    }

    cmd = QString("HM%1\r").arg(static_cast<char>(axis + 'A'));
    cmdV(cmd);
    cmd = QString("BG%1\r").arg(static_cast<char>(axis + 'A'));
    cmdV(cmd);
}

int DMC_B140_M::readTheoryPos(unsigned short axis)
{
    QString cmd = QString("TD%1\r").arg(static_cast<char>(axis + 'A'));
 //   qDebug() << cmd;
    int value = 0;
    cmdI(cmd, &value);
    return value;
}

bool DMC_B140_M::readTheoryPosAll(int *value)
{
    QString cmd = QString("TD\r");
    if( cmdI4(cmd, value) )
    {
        for(int i = 0; i < 4; i++)
        {
            m_TheoryPos[i] = value[i];
        }
        return true;
    }
    return false;
}

void DMC_B140_M::setStartSpeed(unsigned short axis, int startSpeed)
{
    //TODO: 暂时没有实现这个功能
    Q_UNUSED(axis);
    Q_UNUSED(startSpeed);
}

void DMC_B140_M::setStopSpeed(unsigned short axis, int stopSpeed)
{
    //TODO: 暂时没有实现这个功能
    Q_UNUSED(axis);
    Q_UNUSED(stopSpeed);
}

int DMC_B140_M::readEncodePos(unsigned short axis)
{
    QString cmd = QString("TP%1\r").arg(static_cast<char>(axis + 'A'));
    int value = 0;
    cmdI(cmd, &value);
    return value;
}

bool DMC_B140_M::readEncodePosAll(int *value)
{
    QString cmd = QString("TP\r");
    if( cmdI4(cmd, value) )
    {
        for(int i = 0; i < 4; i++)
        {
            m_EncodePos[i] = value[i];
        }
        return true;
    }
    return false;
}

bool DMC_B140_M::readIO(bool *value)
{
    QString cmd = QString("TI\r");
    if( cmdB8(cmd, value) )
    {
        for(int i = 0; i < 8; i++)
        {
            m_IO[i] = value[i];
        }
        return true;
    }
    return false;
}

DMC_B140_M::~DMC_B140_M()
{
    abort();
    disconnect(m_socket, SIGNAL(disconnected()), this, SLOT(disconnected()));
    m_socket->disconnectFromHost();
}

void DMC_B140_M::setTheoryPos(unsigned short axis, long offset)
{
    QString cmd = QString("DP%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(offset);
    cmdV(cmd);
}

void DMC_B140_M::setEncoderPos(unsigned short axis, long offset)
{
    QString cmd = QString("DE%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(offset);
    cmdV(cmd);
}


void DMC_B140_M::connectViaEthernet(QString ip, int port)
{
    setNetworkPara(ip, port);
    if(!m_isInit && !m_ipAddress.isEmpty())
    {
        m_socket->connectToHost(m_ipAddress, m_portNum, QTcpSocket::ReadWrite);
    }
}

void DMC_B140_M::setNetworkPara(QString ip, int port)
{
    m_ipAddress = ip;
    m_portNum = port;
}

void DMC_B140_M::init()
{
/// TODO: 这里应该填加些操作。
}

void DMC_B140_M::initBasicPara()
{
    qDebug() << "B140 connect ok!";

    m_isInit = true;
    cmdV("LD0,0,0,0\r");
    cmdV("SH\r");
    cmdV("CN -1, 1, -1, 0, 0\r");
    cmdV("SB1\r");
    cmdV("SD1073740800,1073740800,1073740800,1073740800\r");//限位停速度
    setAcceleration(0, 256000 * 20);
    setAcceleration(1, 256000 * 2);
    setAcceleration(2, 256000 * 2);
    setAcceleration(3, 256000 * 2);
    setDeceleration(0, 256000 * 20);
    setDeceleration(1, 256000 * 2);
    setDeceleration(2, 256000 * 2);
    setDeceleration(3, 256000 * 2);

    setSpeed(1, 10000);
    setSpeed(2, 10000);
    setSpeed(3, 10000);
    setSpeed(0, 10000);

    emit connectionSetup();
}

void DMC_B140_M::disconnected()
{
    m_isInit = false;
    emit connectionLost();
}

void DMC_B140_M::setSpeed(unsigned short axis, int speed)
{
    m_speed[axis] = speed;
    QString cmd = QString("SP%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(speed);
    //qDebug () << cmd;
    cmdV(cmd);
}

/**
 * @brief DMC_B140_M::setAcceleration
 * @param axis
 * @param acceleration 为无符号整数，范围1024~1073740800，单位是脉冲每秒平方。
 *                     输入的数据如不是1024的倍数，回自动变换为不大于输入数值的1024倍数。
 *                     如输入的数据小于1024，则自动变换为1024。
 */
void DMC_B140_M::setAcceleration(unsigned short axis, int acceleration)
{
    QString cmd = QString("AC%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(acceleration);
//    qDebug() << cmd;
    cmdV(cmd);
}

/**
 * @brief DMC_B140_M::setDeceleration
 * @param axis
 * @param deceleration 为无符号整数，范围1024~1073740800，单位是脉冲每秒平方。
 *                     输入的数据如不是1024的倍数，回自动变换为不大于输入数值的1024倍数。
 *                     如输入的数据小于1024，则自动变换为1024。
 */
void DMC_B140_M::setDeceleration(unsigned short axis, int deceleration)
{
    QString cmd = QString("DC%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(deceleration);
//    qDebug() << cmd;
    cmdV(cmd);
}

void DMC_B140_M::moveTo(unsigned short axis, int position)
{
    QString cmd = QString("PA%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(position);
    cmdV(cmd);

    cmd = QString("BG%1\r").arg(static_cast<char>(axis + 'A'));
    cmdV(cmd);
}

void DMC_B140_M::move(unsigned short axis, int dir)
{
    QString cmd;
    if(dir == 1)
    {
        cmd = QString("JG%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(m_speed[axis]);
    }
    else
    {
        cmd = QString("JG%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(-m_speed[axis]);
    }

    cmdV(cmd);

    cmd = QString("BG%1\r").arg(static_cast<char>(axis + 'A'));
    cmdV(cmd);
}

void DMC_B140_M::moveSteps(unsigned short axis, int steps)
{
    QString cmd = QString("PR%1=%2\r").arg(static_cast<char>(axis + 'A')).arg(steps);
    cmdV(cmd);
    cmd = QString("BG%1\r").arg(static_cast<char>(axis + 'A'));
    cmdV(cmd);
}

void DMC_B140_M::stop(unsigned short axis)
{
    QString cmd;
    if(0<= axis && axis <= 3)
    {
        cmd = QString("ST%1\r").arg(static_cast<char>(axis + 'A'));
    }
    else
    {
        cmd = QString("ST\r");
    }
    cmdV(cmd);
//    qDebug() << cmd;
}

void DMC_B140_M::abort()
{
    QString cmd = QString("AB\r");
    cmdV(cmd);
}

void DMC_B140_M::setEmergencyInput(bool on)
{
    QString cmd;
    if(on)
    {
        cmd = QString("AX1\r");
    }
    else
    {
        cmd = QString("AX0\r");
    }
    cmdV(cmd);
}

